#include "ScnsGyro.c.h"

#if defined(SCNS_GYRO_ENABLE)&&SCNS_GYRO_ENABLE==1

#include "ScnsMenu.h"
#include "ScnsSmctpExtOscilloscope.h"
#include "ScnsIcm20602.h"
#include "ScnsIcm42605.h"

ScnsStatus scnsBspGyroInit(ScnsGyroEnum gyroN)
{
    return SCNS_STATUS_OK;
}

ScnsStatus scnsBspGyroPostInit(ScnsGyroEnum gyroN)
{
//    scnsMenuSetPage(4,"gyro",NULL);
//    scnsMenuAddConstStruct(4,scnsGyroResult[SCNS_GYRO_1],pitch,,SCNS_TYPE_FLOAT);
//    scnsMenuAddConstStruct(4,scnsGyroResult[SCNS_GYRO_1],roll,,SCNS_TYPE_FLOAT);
//    scnsMenuAddConstStruct(4,scnsGyroResult[SCNS_GYRO_1],yaw,,SCNS_TYPE_FLOAT);
//    scnsMenuAddConstStruct(4,scnsGyroResult[SCNS_GYRO_1],temp,,SCNS_TYPE_FLOAT);
    return SCNS_STATUS_OK;
}

void scnsBspGyroUpdateResult(ScnsGyroEnum gyroEnum,ScnsGyroResult this)
{
#if defined(SCNS_VOFA_ENABLE)&&SCNS_VOFA_ENABLE==1
    if(scnsMenuCanPageSend(4))
        scnsSmctpExtOscilloscopeSend(this.pitch,this.roll,this.yaw,this.dPitch,this.dRoll,this.dYaw);
#endif
}

ScnsGyroSource scnsBspGyroRead(ScnsGyroEnum gyroEnum)
{
    static const int8 dir[9]={1,1,1,1,1,1,1,1,1};
#if defined(SCNS_ICM42605_ENABLE)&&SCNS_ICM42605_ENABLE==1
    ScnsGyroSource result=scnsIcm42605Read(SCNS_ICM42605_1);
#endif
#if defined(SCNS_ICM20602_ENABLE)&&SCNS_ICM20602_ENABLE==1
    ScnsGyroSource result=scnsIcm20602Read(SCNS_ICM20602_1);
#endif
    scnsGyroSourceDir(result,dir);
    return result;
}

void scnsBspGyroCalibrationBackup(float gyroBiasX,float gyroBiasY,float gyroBiasZ)
{
    printfFL("Backup gyroBiasX:%f,gyroBiasY:%f,gyroBiasZ:%f\r\n",gyroBiasX,gyroBiasY,gyroBiasZ);
}

ScnsStatus scnsBspGyroCalibrationLoad(float*gyroBiasX,float*gyroBiasY,float*gyroBiasZ,float q[4][1])
{
    return SCNS_STATUS_GYRO_CALIBRATION_FAILED;
}

#endif
